

%allows for a remote controlled, simulated, or real obstacle.  

clear all
%simulation=0(no simulation) simulation=1 (simulation)
boats2=0; %If there are two boats, then add a switch3
boats3=0;
RCobstacle=0;

simulation=0;
COLREGSon=0;
if simulation==0
    startBoat_windows(2)
    if boats2==1
            startBoat_windows(1)
    end
    if boats3==1
startBoat_windows(3)
    end
end
    


t=zeros(1,100);
%% intialization
%meters in latitude
metersindeltalat=111043.07209801719;
metersindeltalon=84693.98967674395;



realboatrobot=2;
realboatobstacle=1;
realboatsubmarine=3;

if simulation==0
    originstat=getStatus(realboatrobot); %takeout
    startlat=originstat.lat;
    startlon=originstat.lon;
else
    startlat=40.443476; %This is the xy position that is set as the origin >> need to set to exact lake coordinates
    startlon=-79.946822;
end



% fig2=figure; %plots in latitude and longitude
% axis([startlat-30/metersindeltalat, startlat+30/metersindeltalat, startlon-30/metersindeltalon, startlon+30/metersindeltalon])
% axis('equal')
fig1 = figure;%plots in meters
pause(4)
hold on
axis('equal')
axis([-30 30 -30 30]);
hold on
sub=1;%number of subintervals
goalbehavior=zeros(1,200);
thinkgoalvelocityangle=0;
thinkgoalspdD=0;
maxaccangle=pi/3;
choices=4;%we have 5 choices (1+2*2)
% [maxaccangle]=maxacc(lamda, tau, k, p_gain, , robomagD); %finds the maximum angle capable of accelarating to given the boat dynamics, (constant magnitude of velocity vector),
inalastpoly=zeros(1,3);
collisionT=1000;%variable comes up before usage
% pastcollisionT=1000*ones(3,choices*2+1);
obstaclecost=3e6;
safetyT=1;
whichcolreg=0;
goalreached=0;
rtempx=-139i;%random # (variable comes up brfor usage)
rtempy=2425i;
notsame=100i;
COLREGhistory=zeros(100,2);
waypointstandoff=20;






%% input variables
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[20];
goalposition = [0,60];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=0; % integrate this with goal xpd yspd
obstaclespd=[.01];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi/2;
velocityangle=pi/2; %commentout
goalheading=0;
evasivegoal=0;
%%

%setup for calling real world boat
RCrobotspd=150;
RCobstaclespd=100;

% RCrobotspd=robomag*400;
% RCobstaclespd=obstaclespd*400;


%define the position of robots and obstacles
timeinterval=5; %seconds in a time interval
standoff=3; %3 meter standoff from the goal
achieved=3; % how far (in meters) I am from the submarine in order to claim that I have reached it.
criticaltime=2;

h=[3];%obstacle height, and width, orientation
width=[.5];
n=[1]; %# of circles break obstacle rectangle intp


% goalspdD=sqrt(goalxspdyspd(1)^2+goalxspdyspd(2)^2);
Rh=3; %robot height, width, & # of circles
Rwidth=.5;
Rn=3;

%draw obstacle (not currently updating orientation- but I should be)
obstacleN=length(h);

obdtheta=obstacleangle-pi/2;
diagonal=sqrt(width.^2+h.^2);
diagonaltheta=atan(h./width);
origobstacleR=sqrt((h.^2)./(4.*n.^2)+(width.^2)./4);
distance=2.*sqrt(origobstacleR.^2-(width.^2)./4);
CW=(n-1).*distance+2.*origobstacleR;
centerY1=obstacleY-abs(((h-CW)./2))+origobstacleR;
centerX1=obstacleX+width./2;
hold on
totalcircleN=sum(n);
circnewX=zeros(1,totalcircleN);
circnewY=zeros(1,totalcircleN);
for i=1:obstacleN
    centerY=zeros(1,n(i));
    centerX=centerX1(i)*ones(1,n(i));
    centerY(1:end)=centerY1(i)+distance(i).*(0:n(i)-1);
    circledist=sqrt((centerX-obstacleX(i)).^2+(centerY-obstacleY(i)).^2);
    circletheta=atan((centerY-obstacleY(i))./(centerX-obstacleX(i)));
    circnewX((sum(n(1:i-1))+1):(sum(n(1:i))))=obstacleX(i)+circledist.*cos(obdtheta(i)+circletheta);
    circnewY((sum(n(1:i-1))+1):(sum(n(1:i))))=obstacleY(i)+circledist.*sin(obdtheta(i)+circletheta);
end


%draw circles for robot
Rdiagonal=sqrt(Rwidth^2+Rh^2);  %length of robot's rectangle diagonal
Rdiagtheta=atan(Rh/Rwidth); %angle robot's diagonal is iat
origrobotR=sqrt((Rh^2)/(4*Rn^2)+(Rwidth^2)/4); %radius of circles
distance=2*sqrt(origrobotR^2-(Rwidth^2)/4); %distance circles are at from eachother
CW=(Rn-1)*distance+2*origrobotR;
situation=0;%we haven't yet gotten into a colreg situation
%define the diameter of the obstacle and robot, and then change the size of
%both
newrobotR=10;


%% setup
robomagD=robomag*timeinterval;
obstaclespdD=obstaclespd*timeinterval;
goalspdD=goalspd*timeinterval;
obstaclexspdD=obstaclespdD*cos(obstacleangle);
obstacleyspdD=obstaclespdD*sin(obstacleangle);


% function  [obstacleX,obstacleY,robotX,robotY,goalreached]=crossing5(whilecount,obstacleXActual,obstacleYActual,obstaclespdD,goalposition,goalspdD,robotXActual,robotYActual,velocityangle)
newobstacleR=origobstacleR+origrobotR+robomagD*safetyT; % I am adding a safety precaution to the velocity obstacle.
similarity=round(.5*max(h)/robomagD);


whilecount=0;
while true
    
    
    whilecount=whilecount+1;
    
  
    
    
    %% find where robot is, and obstacle is.
    if simulation==0
        % uncommentout
        tic
        
        disp('getStat 1');
        robotstat = getStatus(realboatrobot);
        disp('stat received');
        fprintf('robotLatitude %g robotLongitude %g robotVelocityAngle',robotstat.lat,robotstat.lon,(-robotstat.compass+90))
        if boats2==1
            obstaclestat = getStatus(realboatobstacle);
        end
        if boats3==1
            submarinestat= getStatus(realboatsubmarine);
            goalposition=[(submarinestat.lon-startlon)*metersindeltalon,(submarinestat.lat-startlat)*metersindeltalat];

        end
        
        robotY=(robotstat.lat-startlat)*metersindeltalat;
        robotX=(robotstat.lon-startlon)*metersindeltalon;
        if boats2==1
            obstacleY=(obstaclestat.lat-startlat)*metersindeltalat;
            obstacleX=(obstaclestat.lon-startlon)*metersindeltalon;
        end
        if boats2==1
            obstacleangle=(-obstaclestat.compass+90)*pi/180; %convert from clockwise to counterclockwise,0 starting at east, not north.  degrees to radians.  
        if obstacleangle<-pi
            obstacleangle=obstacleangle+2*pi;
        end
        end
        velocityangle=(-robotstat.compass+90)*pi/180;
        if velocityangle<-pi
            velocityangle=obstacleangle+2*pi;
        end
        
        
        %         fig2;
        %         hold on
        %         scatter(robotstat.lat,robotstat.lon);
        %         scatter(startlon+goalposition(2)/metersindeltalon,startlat+goalposition(1)/metersindeltalat)
        %         scatter(startlon+obstacleY/metersindeltalon,startlat+obstacleX/metersindeltalat)
        %         hold off
        %
        %         fig1;
        %         hold on
        
        
        %need to get compass (convert from degrees to radians >>velocityangle), latitude/longitude (and from that
        %derive cartesian coordinates for program)
    end
    
    
    
    %% planning
    
    
    
    
    if velocityangle<=0 && velocityangle>=-pi
        robdtheta=velocityangle+pi;
    else
        robdtheta=velocityangle-pi;
    end
    
    robtargd=sqrt((robotX-goalposition(1))^2+(robotY-goalposition(2))^2);
    
    
    if robtargd<robomagD || robtargd < (achieved + standoff)
        goalreached=1;
        break
    end
    
    Xsmall=robotX; %this function finds the angle btwn 2 pts, adn puts it in the right quadrant
    Xbig=goalposition(1);
    Ysmall=robotY;
    Ybig=goalposition(2);
    theangle=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
    robgoalangle=theangle;
    
    standoffgoalp=goalposition-standoff*[cos(robgoalangle),sin(robgoalangle)];
    
    scatter(standoffgoalp(1),standoffgoalp(2));
    
    robtargd=round(robtargd);
    previousub=sub;
    sub=(robtargd/robomagD);% as distance btwn robot and target increases, so does prediction.  I always want to predict to the earliest pt I can intersect- distance/velocity gives me time.
    if previousub==sub
        sub=1;
    end
    goalscatter = scatter(goalposition(1),goalposition(2));
    submarine=text(goalposition(1),goalposition(2),' \leftarrow submarine','FontSize',10);
    %the positions for each of the roboot circles
    centerY1=robotY-abs(((Rh-CW)/2))+origrobotR;
    centerX1=robotX+Rwidth/2;
    centerX=centerX1*ones(1,Rn);
    circledistance=zeros(1,Rn);
    for circleN=0:(Rn-1)
        circledistance(circleN+1)=distance*circleN;
    end
    centerY=centerY1+circledistance;
    circledist=sqrt((centerX-robotX).^2+(centerY-robotY).^2);
    %The angle formed btwn a corner of the rectangle and the circles
    Xsmall=robotX; %this function finds the angle btwn 2 pts, adn puts it in the right quadrant
    Xbig=centerX;
    Ysmall=robotY;
    Ybig=centerY;
    theangle=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
    circletheta=theangle;
    %     circletheta=atan((centerY-robotY)./(centerX-robotX)); %need to put in right quadrant later
    robotcircleX=robotX+circledist.*cos(robdtheta-pi/2+circletheta); %circle positions
    robotcircleY=robotY+circledist.*sin(robdtheta-pi/2+circletheta);
    
    %drawing the robot/obstacle circles
    boat=zeros(1,obstacleN);
    for i=1:obstacleN
        boatXrotated=[obstacleX(i),obstacleX(i)+width(i)*cos(obdtheta(i)),obstacleX(i)+diagonal(i)*cos(obdtheta(i)+diagonaltheta(i)),obstacleX(i)+h(i)*cos(obdtheta(i)+pi/2)];
        boatYrotated=[obstacleY(i),obstacleY(i)+width(i)*sin(obdtheta(i)),obstacleY(i)+diagonal(i)*sin(obdtheta(i)+diagonaltheta(i)),obstacleY(i)+h(i)*sin(obdtheta(i)+pi/2)];
        boat(i)= patch(boatXrotated,boatYrotated,'r');
    end
    robotXrotated=[robotX,robotX+Rwidth*cos(robdtheta-pi/2),robotX+Rdiagonal*cos(robdtheta-pi/2+Rdiagtheta),robotX+Rh*cos(robdtheta)];
    robotYrotated=[robotY,robotY+Rwidth*sin(robdtheta-pi/2),robotY+Rdiagonal*sin(robdtheta-pi/2+Rdiagtheta),robotY+Rh*sin(robdtheta)];
    robot= patch(robotXrotated,robotYrotated,'b');
    
    robotscatter=scatter(robotcircleX,robotcircleY,newrobotR); %b/ the robot is now 'point-sized' I draw small dots in the center of where the circles would be.
    robotarrow=text(robotX,robotY,' \leftarrow USV','FontSize',10);
    % for whichobstacle=1:obstacleN
    %         for whichcirc=(sum(n(1:whichobstacle-1))+1):(sum(n(1:whichobstacle))) %Draw obstacle circles
    %             obstacle(whichcirc)=rectangle('Position',[circnewX(whichcirc)-newobstacleR(whichobstacle),circnewY(whichcirc)-newobstacleR(whichobstacle),newobstacleR(whichobstacle)*2,newobstacleR(whichobstacle)*2],'Curvature',[1 1]);
    %         end
    %     end
    
    choicematrix=zeros(5,9); %I will fill this matrix w/ the choices I have
    %     remainder=rem(whilecount,sub); %so, I predict where the goal will be at the end of the large interval.  If I am on the first sub interval, and there are 4 sub intervals, then I need to multiply the velocity vector of the goal by 3, adn add it to the position of the goal.  If 2, multiply by 2.  If 3, multiply by one.
    %     if remainder==0
    %         remainder=sub;
    %     end
    if isnan(thinkgoalvelocityangle)
        predictgoal=standoffgoalp;
    else
        predictgoal=standoffgoalp+[cos(thinkgoalvelocityangle)*thinkgoalspdD, sin(thinkgoalvelocityangle)*thinkgoalspdD].*sub;%predicted goal position
    end
    robomagDs=[robomagD,robomagD];
    prevvelocityangle=velocityangle;
    previousvelocityangles(whilecount)=velocityangle;
    
    %% commentout
    if simulation==1
        if whilecount==1 %since robot is at an initial orientation when I start, I make the velocity vector pi+the amount of degrees it is at.
            
            
        else %I find the angle of the velocity vector (after the 1st time) by finding the angle btwn the last robot position adn current robot position
            Xsmall=rtempx;
            Xbig=robotX;
            Ysmall=rtempy;
            Ybig=robotY;
            theangle=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
            velocityangle=theangle;
        end
    end
    %%
    
    %The different columns in the matrix are as follows- 1st 2 columns- xy
    %position at end of velocity vector.  2nd two columns, distance gone at
    %end of velocity vector.  Last 2 columns, the distance from the actual
    %goal, and the distance from the predicted goal.
    %The rows are divided into five paths, 2 paths on the side of the
    %maximum acceleration angle, that divided by 2, and the current
    %velocity.
    accangles= (-maxaccangle:maxaccangle/(choices):maxaccangle)';
    
    robdthetachoices=velocityangle+accangles;
    choicesR=1:2;
    for i=1:length(robomagDs)
        for ii=1:(choices*2+1)
            choicematrix(ii,choicesR)=[robotX+robomagDs(i)*cos(robdthetachoices(ii)),robotY+robomagDs(i)*sin(robdthetachoices(ii))];
        end
        choicesR=choicesR+2;
    end
    choicematrix(:,5)=robdthetachoices;
    choicematrix(:,10)=accangles;
    %now do the rest only for inpoly
    
    %     for i=1:5
    %         scatter(choicematrix(i,3),choicematrix(i,4))
    %     end
    
    %     robobd=sqrt((robotX-obstacleX)^2+(robotY-obstacleY)^2);
    %now the velocity obstacle portion (avoiding the obstacle)
    %all matrices I am going to fill in later
    
    CCx=zeros(Rn*totalcircleN,3);
    CCy=zeros(Rn*totalcircleN,3);
    
    VOpatch=zeros(Rn*totalcircleN,1);
    CCpatch=zeros(Rn*totalcircleN,1);
    thetadif=zeros(1,totalcircleN);
    tanline=zeros(1,totalcircleN);
    endCC=0;
    centerangle=atan(abs((circnewY-robotY))./abs((circnewX-robotX))); %angle between a certain robot circle, and the center of the obstacle
    distancerobob=sqrt((circnewX-robotX).^2+(circnewY-robotY).^2); %distance from robot to obstacle
    for whichclmn=1:totalcircleN
        if circnewX(whichclmn)>robotX %4the quadrand
            if circnewY(whichclmn)<robotY
                centerangle(whichclmn)=-centerangle(whichclmn);
            end
        else  %second,or third quadrant
            if circnewY(whichclmn)>robotY%second quadrant
                centerangle(whichclmn)=pi-centerangle(whichclmn);
            else
                centerangle(whichclmn)=centerangle(whichclmn)-pi; %third quadrant
            end
        end
    end
    for whichobstacle=1:obstacleN
        startclmn=(sum(n(1:whichobstacle-1))+1);
        endclmn=(sum(n(1:whichobstacle)));
        thetadif(startclmn:endclmn)=asin(newobstacleR(whichobstacle)./distancerobob(startclmn:endclmn)); %the angle drawn from these three points- 1.  The robot circle 2.  The center of the obstacle circle 3.the point of intersection between a line tangent to the obstacle circle going through the robot circle and the obstacle circle
        tanline(startclmn:endclmn)=sqrt((distancerobob(startclmn:endclmn)).^2-newobstacleR(whichobstacle)^2); %the distance from the robotcircle to the tangent-obstaclecircle line intersection
    end
    theta1=centerangle+thetadif; %I add and subtract this angle from the angle formed between the robot circle and the center of the obstacle circle
    theta2=centerangle-thetadif;
    intersection1X=robotX+tanline.*cos(theta1); %the top x intersection pt
    intersection1Y=robotY+tanline.*sin(theta1); %the top y intersection pt
    intersection2X=robotX+tanline.*cos(theta2); %the bottom x intersection pt
    intersection2Y=robotY+tanline.*sin(theta2); % the bottom y intersection pt
    for whichrobot=1:Rn
        startCC=endCC+1;
        endCC=startCC+totalcircleN-1;
        robotcircleXcolumn=robotcircleX(whichrobot)*ones(1,totalcircleN); %I place the intersection points versus which robot-circle I am currently on in a matrix
        robotcircleYcolumn=robotcircleY(whichrobot)*ones(1,totalcircleN);
        CCx(startCC:endCC,:)=[robotcircleXcolumn;intersection1X;intersection2X]'; %the absolute value is so that when I sort the matrix, the negative values will not come before the zeros.
        CCy(startCC:endCC,:)=[robotcircleYcolumn;intersection1Y;intersection2Y]';
        CCx=real(CCx);
        CCy=real(CCy);
        for whichobstacle=1:obstacleN
            startclmn=(whichrobot-1)*totalcircleN+(sum(n(1:whichobstacle-1))+1);
            endclmn=(whichrobot-1)*totalcircleN+(sum(n(1:whichobstacle)));
            VOx(startclmn:endclmn,:)=CCx(startclmn:endclmn,:)+obstaclexspdD(whichobstacle);
            VOy(startclmn:endclmn,:)=CCy(startclmn:endclmn,:)+obstacleyspdD(whichobstacle);
        end
    end
    alpha(.0001);
    
    
    inapoly=0;
    choicematrix(:,8)=sqrt((choicematrix(:,3)-predictgoal(1)).^2+(choicematrix(:,4)-predictgoal(2)).^2);%predicthcost
    choicematrix(:,7)=sqrt((choicematrix(:,3)-standoffgoalp(1)).^2+(choicematrix(:,4)-standoffgoalp(2)).^2);%hcost
    collisionT=abs((obstacleY-robotY)./(robomagD.*sin(velocityangle)-obstacleyspdD));
    collisioncheckT=abs((obstacleX-robotX)./(robomagD.*cos(velocityangle)-obstaclexspdD));
    if min(collisionT)<min(collisioncheckT)
        mincollisionT=min(collisionT);
        check=0;
    else
        mincollisionT=min(collisioncheckT);
        check=1;
    end
    if robtargd< (min(sqrt((robotX-obstacleX).^2+(robotY-obstacleY).^2)))*1.5
    else
        for choice=1:(choices*2+1)
            checkdeadlock=0;
            inpoly=0; %checks whether velocity vector is in velocity obstacle.  If it is, then give it a cost of 300 thousand, so we don't go there.
            choicecollisionT=abs((obstacleY+obstacleyspdD-choicematrix(choice,4))./(robomagD.*sin(velocityangle)-obstacleyspdD));
            choicecollisioncheckT=abs((obstacleX+obstaclexspdD-choicematrix(choice,3))./(robomagD.*cos(velocityangle)-obstaclexspdD));
            if min(choicecollisionT)<min(choicecollisioncheckT)
                choicemincollisionT=min(choicecollisionT);
                check=0;
            else
                choicemincollisionT=min(choicecollisioncheckT);
                check=1;
            end
            for patchVO=1:totalcircleN*Rn
                in=inpolygon(choicematrix(choice,1),choicematrix(choice,2),VOx(patchVO,:),VOy(patchVO,:));
                if in==1 || in==.5%if I didn't do this, it would just see whether the last value of in is 1.  So, if any of the values of in are 1, it moves on, b/ there is obviously an obstacle.
                    inpoly=1;
                    break
                end
            end
            if min(sqrt((choicematrix(choice,3)-(obstacleX+obstaclexspdD)).^2+(choicematrix(choice,4)-(obstacleY+obstacleyspdD)).^2))<criticaltime*(robomagD)
                choicematrix(choice,7:8)=choicematrix(choice,7:8)+obstaclecost;
                inapoly=1;
            else
                if inpoly==1&& choicemincollisionT<criticaltime
                    if check==0
                        [C,I]=min(choicecollisionT);
                    else
                        [C,I]=min(choicecollisioncheckT);
                    end
                    if abs(choicecollisioncheckT(I)-choicecollisionT(I))<criticaltime
                        choicematrix(choice,7:8)=choicematrix(choice,7:8)+obstaclecost;
                        inapoly=1;
                    else
                        checkdeadlock=1;
                    end
                else
                    checkdeadlock=1;
                end
            end
            if checkdeadlock==1
                Xsmall=robotX;
                Xbig=choicematrix(choice,3);
                Ysmall=robotY;
                Ybig=choicematrix(choice,4);
                theangle=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
                nextvelocityangle=theangle;
                nextrobdthetachoices=nextvelocityangle+accangles;
                nextchoices=zeros(choices*2+1,2);
                for ii=1:(choices*2+1)
                    nextchoices(ii,1:2)=[choicematrix(choice,3)+robomagD*cos(nextrobdthetachoices(ii)),choicematrix(choice,4)+robomagD*sin(nextrobdthetachoices(ii))];
                end
                nextrobotXY=[robotcircleX+sqrt((robotX-choicematrix(choice,3)).^2+(robotY-choicematrix(choice,4)).^2)*cos(nextvelocityangle);robotcircleY+sqrt((robotX-choicematrix(choice,3)).^2+(robotY-choicematrix(choice,4)).^2).*sin(nextvelocityangle)]'; %I might be using the wrong angle.  maybet it is nextvelocityangle-180 or 90 or something
                %intersectionX1Y1X2Y2=[intersection1X+obstaclexspdD;intersection1Y+obstacleyspdD;intersection2X+obstaclexspdD;intersection2Y+obstacleyspdD];
                for whichrobot=1:Rn
                    startCC=endCC+1;
                    endCC=startCC+totalcircleN-1;
                    nextCCx(startCC:startCC+totalcircleN-1,:)=[nextrobotXY(whichrobot,1)*ones(1,totalcircleN);intersection1X;intersection2X]'; %the absolute value is so that when I sort the matrix, the negative values will not come before the zeros.
                    nextCCy(startCC:startCC+totalcircleN-1,:)=[nextrobotXY(whichrobot,2)*ones(1,totalcircleN);intersection1Y;intersection2Y]';
                    for whichobstacle=1:obstacleN
                        startclmn=(whichrobot-1)*totalcircleN+(sum(n(1:whichobstacle-1))+1);
                        endclmn=(whichrobot-1)*totalcircleN+(sum(n(1:whichobstacle)));
                        nextVOx(startclmn:endclmn,:)=CCx(startclmn:endclmn,:)+[obstaclexspdD(whichobstacle)*ones(1,endclmn-startclmn+1);2*obstaclexspdD(whichobstacle)*ones(1,endclmn-startclmn+1);2*obstaclexspdD(whichobstacle)*ones(1,endclmn-startclmn+1)]';
                        nextVOy(startclmn:endclmn,:)=CCy(startclmn:endclmn,:)+[obstacleyspdD(whichobstacle)*ones(1,endclmn-startclmn+1);2*obstaclexspdD(whichobstacle)*ones(1,endclmn-startclmn+1);2*obstacleyspdD(whichobstacle)*ones(1,endclmn-startclmn+1)]';
                    end
                end
                nextinpoly=zeros(1,choices*2+1); %checks whether velocity vector is in velocity obstacle.  If it is, then give it a cost of 300 thousand, so we don't go there.
                for nextchoice=1:(choices*2+1)
                    for patchVO=1:totalcircleN*Rn
                        in=inpolygon(nextchoices(nextchoice,1),nextchoices(nextchoice,2),nextVOx(patchVO,:),nextVOy(patchVO,:));
                        if in==1 || in==.5%if I didn't do this, it would just see whether the last value of in is 1.  So, if any of the values of in are 1, it moves on, b/ there is obviously an obstacle.
                            nextinpoly(1,nextchoice)=1;
                            break
                        end
                    end
                    if min(sqrt((obstacleX+2*obstaclexspdD-nextchoices(choice,1)).^2+(obstacleY+2*obstacleyspdD-+nextchoices(choice,2)).^2))<criticaltime*robomagD
                        nextinpoly(1,nextchoice)=1;
                    end
                end
                if sum(nextinpoly)==5
                    choicematrix(choice,7:8)=choicematrix(choice,7:8)+obstaclecost;
                end
            end
        end
    end
    
    COLREG=0;
    if whilecount==9
        as=5;
    end
    if (inapoly==1 )||sum(inalastpoly)>0
        prevwhichcolreg=whichcolreg;
        
        [~,whichcolreg]=min(collisionT);
        
        checkcolreg=0;
        if (prevwhichcolreg==whichcolreg )||(sum(inalastpoly)>0 )%& notsame~=(whilecount-1)||inapoly==0 %If the obstacle I wanted to avoid in the last time interval is the same as the one I want to avoid in this time interval, or if i have been in the velocity obstacle recently
            if notsame~=(whilecount-1)||inapoly==0  %but currently not in VO or I did not just change which obstacle I want to avoid suddenly
                
                %question: how can prevwhichcollreg=which colreg, but the
                %obstacle I want avoid changes suddenly.
                COLREG=COLREGhistory(whilecount-1,1);
            else
                checkcolreg=1;
            end
        else
            checkcolreg=1;
        end
        if checkcolreg==1
            relativeVangle=velocityangle-obstacleangle;
            displacementangle=zeros(1,obstacleN);
            for i=1:obstacleN
                Xsmall=obstacleX(i);
                Xbig=robotX;
                Ysmall=obstacleY(i);
                Ybig=robotY;
                displacementangle(i)=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
            end
            situationangle=displacementangle+relativeVangle;
            %         if situation==1 %if we have already gotten into a colreg situation, then stay w/ that colreg. else, choose what the colreg situation is.
            %             fprintf('whilecount=%g',whilecount)
            %         else
            %             for whichobstacle=1:obstacleN
            if situationangle(whichcolreg)>pi
                situationangle(whichcolreg)=situationangle(whichcolreg)-2*pi;
            end
            if situationangle(whichcolreg)<-pi
                situationangle(whichcolreg)=situationangle(whichcolreg)+2*pi;
            end
           
            
            if situationangle(whichcolreg)>=-pi/4 && situationangle(whichcolreg)<=pi/4
                COLREG=4;%crossleft
            end
            if situationangle(whichcolreg)>-3*pi/4 && situationangle(whichcolreg)<-pi/4
                COLREG=1; %overtake
            end
            if (situationangle(whichcolreg)>=3*pi/4 && situationangle(whichcolreg)<=pi)||(situationangle(whichcolreg)>=-pi && situationangle(whichcolreg)<=-3*pi/4)
                COLREG=2;%crossright
            end
            if situationangle(whichcolreg)>pi/4 && situationangle(whichcolreg)<3*pi/4
                COLREG=3;%headon
            end
        end
    else
        whichcolreg=0;
    end
    
    
    
    if (whilecount-similarity-1>0)&& (sum(COLREGhistory(whilecount-similarity-1:whilecount-1,2))==(similarity+1)*COLREGhistory(whilecount-1,2)) && whichcolreg~=COLREGhistory(whilecount-1,2) %if it has been the same obstacle for "similarity"-period of time, and all of a sudden teh obstacle changes, continue with the previous situation
        COLREG=COLREGhistory(whilecount-1,1);
        notsame=whilecount;
    end
    
  if COLREGSon==0
      COLREG=0;
  end
    %     if whichcolreg>0 && sign(velocityangle)==sign(displacementangle(whichcolreg))
    % %     else
    % % %         if sign(cos(velocityangle-displacementangle))==-1 %if the obstacle is behind the robot, then I don't want to do a COLREG
    % % %             COLREG=0; % I think this is wrong- later go back and see.
    % % %         end
    % %         if sign(velocityangle)==sign(displacementangle(whichcolreg))
    % %             COLREG=0;
    % %         end
    %     end
    goalcollisionT=abs((standoffgoalp(2)-robotY)./(robomagD.*sin(velocityangle)-sin(thinkgoalvelocityangle)*thinkgoalspdD));
    goalcollisioncheckT=abs((standoffgoalp(1)-robotX)./(robomagD.*cos(velocityangle)-cos(thinkgoalvelocityangle)*thinkgoalspdD));
    if abs(goalcollisionT-goalcollisioncheckT)<criticaltime
        if goalcollisionT<min(collisionT)|| goalcollisioncheckT<min(collisionT)
            COLREG=0;
        end
    end
    if robtargd<min(sqrt((robotX-obstacleX).^2+(robotY-obstacleY).^2))
        COLREG=0;
    end
    COLREGhistory(whilecount,:)=[COLREG,whichcolreg];
    
    
    choicematrix=sortrows(choicematrix,10);
    if COLREG==1
        textcolreg=text(robotX,robotY,'overtake','FontSize',20); %overtake
    end
    if COLREG==2
        textcolreg=text(robotX,robotY,'crossright','FontSize',20); %crossright
    end
    if COLREG==3
        textcolreg=text(robotX,robotY,'headon','FontSize',20); %headon
    end
    if COLREG==4
        textcolreg=text(robotX,robotY,'crossleft','FontSize',20); %crossleft
    end
    
    %for tthe left choices, accangles are positive.  For the right choices,
    %accangles are negative
    if COLREG==1||COLREG==2||COLREG==3 %if you wanna go right
        choicematrix(choices+1:2*choices+1,7:8)=choicematrix(choices+1:2*choices+1,7:8)+obstaclecost/10;
    end
    if COLREG==4
        choicematrix(1:choices+1,7:8)=choicematrix(1:choices+1,7:8)+obstaclecost/10;
    end
    if similarity>0
        if whilecount>1
            if (whilecount-1)<similarity
                inapolyremainder=whilecount-1;
            else
                inapolyremainder=rem(whilecount-1,similarity);
            end
            if inapolyremainder==0
                inapolyremainder=similarity;
            end
            inalastpoly(inapolyremainder)=inapoly;
        end
    end
    
    
    rtemptempx=rtempx; %position of robot 2 time intervals ago
    rtemptempy=rtempy; %position of robot last time
    rtempx=robotX; % current position fo the robot
    rtempy=robotY;
    rtempxrtempy=[rtempx rtempy];
    
    %     if hcost(1)<2*robomagD %if close to the goal, switch to pure pursuit
    %         choicematrix=sortrows(choicematrix,7);
    %     else
    choicematrix=sortrows(choicematrix,8);
    %=     end
    robdtheta=choicematrix(1,5)+pi; %orientation of robot is the velocity vector rotated pi radians
    %     scatter(predictgoal(1),predictgoal(2))
    %     for i=1:5
    %         scatter(choicematrix(i,3),choicematrix(i,4))
    %     end
    scatter(obstacleX,obstacleY,10,'black')
    scatter(robotX,robotY,10,'r')
    %here you put in the boat model and control (rtempx and rtempy is your
    %actual position, robotX/robotY is your desired position.
    
    %update positions of everything
    desiredrobotX=choicematrix(1,3);
    desiredrobotY=choicematrix(1,4);
    desiredobstacleX=obstacleX+obstaclexspdD;
    desiredobstacleY=obstacleY+obstacleyspdD;
    
    
    
    for whichobstacle=1:obstacleN
        startclmn=(sum(n(1:whichobstacle-1))+1);
        endclmn=(sum(n(1:whichobstacle)));
        circnewX(startclmn:endclmn)=circnewX(startclmn:endclmn)+obstaclexspdD(whichobstacle);
        circnewY(startclmn:endclmn)=circnewY(startclmn:endclmn)+obstacleyspdD(whichobstacle);
    end
    Xsmall=robotX;
    Xbig=goalposition(1);
    Ysmall=robotY;
    Ybig=goalposition(2);
    robgoaldispangle=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
    previousgoalposition=goalposition;
    
    if evasivegoal==1
        goalchoiceN=8;
        goalanglechoice=goalspdD*[cos(0:2*pi/goalchoiceN:2*pi)',sin(0:2*pi/goalchoiceN:2*pi)'];
        goalchoices=zeros(goalchoiceN,2);
        robgolchoiceD=zeros(1,length(goalchoices(:,1)));
        for i=1:length(goalanglechoice(:,1))
            goalchoices(i,:)=goalposition+goalanglechoice(i,:);
            robgolchoiceD(i)=sqrt(sum((goalchoices(i,:)-[robotX,robotY]).^2));
        end
        [~,maxDindex]=max(robgolchoiceD);
        goalposition=goalchoices(maxDindex,:);
        goalheading=2*pi/goalchoiceN*maxDindex;
    end
    
    goalposition=goalposition+goalspdD*[cos(goalheading),sin(goalheading)];
    
    Xsmall=previousgoalposition(1);
    Xbig=goalposition(1);
    Ysmall=previousgoalposition(2);
    Ybig=goalposition(2);
    theangle=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
    thinkgoalvelocityangle=theangle;
    
    if isnan(thinkgoalvelocityangle)
        thinkgoalvelocityangle=0;
    end
    
    thinkgoalspdD=sqrt((goalposition(1)-previousgoalposition(1))^2+(goalposition(2)-previousgoalposition(2))^2);
    
    %% go where I wanna go . 
    [Xsmall,Xbig,Ysmall,Ybig]=deal(robotX,desiredrobotX,robotY,desiredrobotY);
    wayptstandangleR=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
    waypointstandR=[robotX,robotY]+robomag*(timeinterval+waypointstandoff)*[cos(wayptstandangleR),sin(wayptstandangleR)];
    
    [Xsmall,Xbig,Ysmall,Ybig]=deal(obstacleX,desiredobstacleX,robotY,desiredobstacleY);
    wayptstandangleO=rightquadrant(Xsmall,Ysmall,Xbig,Ybig);
    waypointstandO=[obstacleX,obstacleY]+obstaclespd*(timeinterval+waypointstandoff)*[cos(wayptstandangleO),sin(wayptstandangleO)];
    
    desiredRoblon=startlon+(waypointstandR(1))/metersindeltalon;
    desiredRoblat=startlat+waypointstandR(2)/metersindeltalat;
    
    desiredObstaclelon=startlon+(waypointstandO(1))/metersindeltalon;
    desiredObstaclelat=startlat+waypointstandO(2)/metersindeltalat;
    pause(.5)
    if simulation==0
        disp('send command1');
        sendCommand(realboatrobot, 'goAtSpeed',desiredRoblat, desiredRoblon,RCrobotspd);
        disp('command sent');
        scatter(desiredrobotX,desiredrobotY,'b');
        
        fprintf('desiredrobotX= %g desiredrobotY %g /n robotX= %g robotY= %g',desiredrobotX,desiredrobotY,robotX,robotY)
        if RCobstacle==1 && boats2==1
            
            sendCommand(realboatobstacle, 'goAtSpeed',desiredObstaclelat ,desiredObstaclelon,RCobstaclespd);
            
        end
    end
    if simulation==1
        robotX=desiredrobotX;
        robotY=desiredrobotY;
        obstacleX=desiredobstacleX;
        obstacleY=desiredobstacleY;
    end
    if simulation==0
    t=toc;
    if t<timeinterval
        pausetime=timeinterval-t;
    else
        pausetime=0;
    end
    end
    if simulation==0
    pause(pausetime)
    else
        pause(1)
    end
%   if simulation==0
%       if pausetime>.5
%       for i=1:round(pausetime/.5)
% %           tic
%           robotstat=getStatus(realboatrobot);
%           
%           obstaclestat=getStatus(realboatobstacle);
%           scatter((robotstat.lon-startlon)*metersindeltalon,(robotstat.lat-startlat)*metersindeltalat,'g')
%           scatter((obstaclestat.lon-startlon)*metersindeltalon,(obstaclestat.lat-startlat)*metersindeltalat,'g')
% 
%           pause(.5)
%       end
%       end
%   else
%   pause(1);
%   end
   
    
    
    %deleting all
    if ishandle(goalscatter)
        delete(goalscatter);
    end
    if COLREG==0
    else
        delete(textcolreg)
    end
    delete(submarine)
    delete(robotscatter);
    %     delete(obstaclescatter);
    %     delete(VOpatch);
    delete(robotarrow);
    %     delete(robot);
    delete(boat);
end
if simulation==0
sendCommand(realboatrobot,'halt')
sendCommand(realboatobstacle,'halt')
end

%  text(robotX,robotY,' \leftarrow robot&submarine','FontSize',20)
% text(obstacleX,obstacleY,' \leftarrow ship-in-the-way','FontSize',20)
